#include <rclcpp/rclcpp.hpp>
#include <chrono>
#include <cstdlib>
#include "gripper_control/srv/gripper_control.hpp"

using namespace std::chrono_literals;

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);

    if (argc != 3) {
        RCLCPP_INFO(rclcpp::get_logger("gripper_client"), "Usage: gripper_client <open|close> <left|right>");
        return 1;
    }

    // auto node = rclcpp::Node::make_shared("gripper_client");
    // auto client = node->create_client<gripper_control::srv::GripperControl>("gripper_control");

    // while (!client->wait_for_service(1s)) {
    //     if (!rclcpp::ok()) {
    //         RCLCPP_ERROR(rclcpp::get_logger("gripper_client"), "Interrupted while waiting for the service. Exiting.");
    //         return 0;
    //     }
    //     RCLCPP_INFO(rclcpp::get_logger("gripper_client"), "Service not available, waiting again...");
    // }

    // auto request = std::make_shared<gripper_control::srv::GripperControl::Request>();
    // if (std::string(argv[1]) == "open") {
    //     request->open = true;
    // } else if (std::string(argv[1]) == "close") {
    //     request->open = false;
    // } else {
    //     RCLCPP_INFO(rclcpp::get_logger("gripper_client"), "Invalid command. Usage: gripper_client <open|close> <left|right>");
    //     return 1;
    // }

    // if (std::string(argv[2]) == "left") {
    //     request->gripper_side = 0;
    // } else if (std::string(argv[2]) == "right") {
    //     request->gripper_side = 1;
    // } else {
    //     RCLCPP_INFO(rclcpp::get_logger("gripper_client"), "Invalid gripper side. Usage: gripper_client <open|close> <left|right>");
    //     return 1;
    // }

    // auto result = client->async_send_request(request);
    // // Wait for the result.
    // if (rclcpp::spin_until_future_complete(node, result) ==
    //     rclcpp::FutureReturnCode::SUCCESS)
    // {
    //     if (request->gripper_side == 0) {
    //         if (result.get()->is_open) {
    //             RCLCPP_INFO(rclcpp::get_logger("gripper_client"), "Left gripper is open.");
    //         } else {
    //             RCLCPP_INFO(rclcpp::get_logger("gripper_client"), "Left gripper is closed.");
    //         }
    //     } else {
    //         if (result.get()->is_open) {
    //             RCLCPP_INFO(rclcpp::get_logger("gripper_client"), "Right gripper is open.");
    //         } else {
    //             RCLCPP_INFO(rclcpp::get_logger("gripper_client"), "Right gripper is closed.");
    //         }
    //     }
    // } else {
    //     RCLCPP_ERROR(rclcpp::get_logger("gripper_client"), "Failed to call service gripper_control");
    // }

    rclcpp::shutdown();
    return 0;
}